/*
 * Copyright (c) 2024 Shenzhen Kaihong Digital Industry Development Co., Ltd.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifdef __cplusplus
extern "C" {
#endif // __cplusplus
#include <stdint.h>
#include <sched.h>
#include <time.h>
#include <unistd.h>
#include "sle_iperf_ssap_client_L2.h"
#include "sle_iperf_common.h"
#include <pthread.h>
#include <inttypes.h>
#include <stdio.h>
pthread_mutex_t tx_mutex;
//data
static tx_context_t tx_thread_tbl[SLE_KHT_MAX_ADDR_RESULTS]; //连接列表
static sle_kht_txrx_stat_t client_rx_stat = {0};  //总数据
static uint64_t sle_kht_this_pack_arrive = 0;
static uint64_t sle_kht_last_pack_arrive = 0;
static uint64_t sle_kht_max_jitter = 0;
static uint16_t g_sle_client_conn_state = 0;
static uint16_t g_connectId_CToS = 0;
static uint16_t g_dataLength_CToS = 460;
static int32_t g_flagIfCmd = 1;
static uint16_t g_SLE_KHT_IPERF_ONCE_PACK_NUM_S_TO_C = SLE_KHT_IPERF_ONCE_PACK_NUM;
static uint16_t g_SEND_STEP_S_TO_C = SEND_STEP;
static uint16_t g_ALL_SEND_PACKAGE_S_TO_C = ALL_SEND_PACKAGE;
static uint32_t g_IPERF_TIME_S_TO_C = IPERF_TIME;
static uint32_t g_iperf_long_time_delay_msS_TO_C = 0;

static uint64_t g_time_delay_start = 0;
static uint64_t g_time_delay_end = 0;
static uint64_t g_time_delay_count = 0;
static uint64_t g_time_delay_send_num = 0;
static uint64_t g_time_delay_send_num_count = 0;

static uint64_t g_time_delay_one_pack_min = 0;
static uint64_t g_time_delay_one_pack_max = 0;
static uint64_t g_time_delay_one_pack_this = 0;
static uint64_t g_time_delay_one_pack_jitter = 0;

static uint16_t g_end_iperf_count = 0;
static uint16_t g_iperfContinueSend = 0;//是否处于打流状态，1表示打流，0表示没有
static uint16_t g_autoConnect_when_discon = 0;//是否自动重连，1表示是，0表示否
static uint64_t g_iperf_rtt_count_connectID[SLE_KHT_MAX_ADDR_RESULTS] = {0};
static uint64_t g_iperf_rtt_start_time = 0;
static uint64_t g_iperf_trr_all_time = 0;
uint64_t sle_kh_max(uint64_t numOne, uint64_t numTwo)
{
    if (numOne >= numTwo) {
        return numOne;
    } else {
        return numTwo;
    }
}

uint64_t sle_kh_min(uint64_t numOne, uint64_t numTwo)
{
    if (numOne > numTwo) {
        return numTwo;
    } else {
        return numOne;
    }
}

// 返回微秒
uint64_t sle_khtest_clock_gettime_us(void)
{
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return ((uint64_t)ts.tv_sec * 1000000ULL + (uint64_t)ts.tv_nsec / 1000ULL);
}

tx_context_t * sle_client_find_valid_ctx_by_connid(uint16_t conn_id)
{
    if (conn_id >= SLE_KHT_MAX_ADDR_RESULTS) return NULL;
    tx_context_t *tx_ctx = &tx_thread_tbl[conn_id];
    if (tx_ctx->valid == 1 && tx_ctx->connect_id == conn_id) {
        return tx_ctx;
    }
    return NULL;
}

uint16_t sle_client_find_valid_ctx_by_mac_addr(uint8_t *addr)
{
    for (int i = 0; i < SLE_KHT_MAX_ADDR_RESULTS; ++i) {
        if (tx_thread_tbl[i].valid == 1 && addr[0] == tx_thread_tbl[i].device_addr[0] &&
                addr[1] == tx_thread_tbl[i].device_addr[1] && addr[2] == tx_thread_tbl[i].device_addr[2] &&
                addr[3] == tx_thread_tbl[i].device_addr[3] && addr[4] == tx_thread_tbl[i].device_addr[4] &&
                addr[5] == tx_thread_tbl[i].device_addr[5]) {
            return 1;
        }
    }
    return 0;
}


uint16_t GetCountConnectedNum(void)
{
    uint16_t countNum = 0;
    for (int i = 0; i < SLE_KHT_MAX_ADDR_RESULTS; ++i) {
        if (tx_thread_tbl[i].valid == 1 && tx_thread_tbl[i].connect_id >= 0){
            countNum++;
        }
    }
    return countNum;
}

void OnSleTNotificatedDelayCb(uint8_t clientId, uint16_t connectId
        , const ClientHandleValue *data, SleErrorCode errCode)
{
    double diff_us = 0;

    if (data->dataLength < sizeof(sle_kht_msghdr)) {
        printf("[SleSsapClientTestTool] OnSleTNotificatedCb: (%d,%d) handle(%d) type(%s) errCode(%hhu) value(%s)\r\n"
                , connectId, clientId, data->handle, SleSsapPropertyTypeStr(data->type), errCode, data->data);
        return;
    }

    sle_kht_msghdr *msghdr = (sle_kht_msghdr *)data->data;
    if (msghdr->magic_num != SLE_KHT_MAGIC_NUM) {
        printf("[SleSsapClientTestTool] OnSleTNotificatedCb: (%d,%d) handle(%d) type(%s) errCode(%hhu) value(%s)\r\n"
                , connectId, clientId, data->handle, SleSsapPropertyTypeStr(data->type), errCode, data->data);
        return;
    }

    tx_context_t *tx_ctx = sle_client_find_valid_ctx_by_connid(connectId);
    if (!tx_ctx) {
        printf("[iperf-test] sle_client_notification_cb, not found conn_id=%u object\n", connectId);
        return;
    }

//    pthread_mutex_lock(&tx_mutex);
    if (msghdr->msgid == COUNT_TIME_DELAY_ID) {
        g_time_delay_end = sle_khtest_clock_gettime_us();
        g_time_delay_one_pack_this = g_time_delay_end - g_time_delay_start; //本次包时延
        g_time_delay_count = g_time_delay_count + g_time_delay_one_pack_this; // 总计时延
        g_time_delay_one_pack_max = sle_kh_max(g_time_delay_one_pack_max, g_time_delay_one_pack_this);//最大时延
        g_time_delay_send_num_count++;//g_time_delay_diff_jitter
        if (g_time_delay_send_num_count == 1) {
            g_time_delay_one_pack_min = g_time_delay_one_pack_this;
        }
        g_time_delay_one_pack_min = sle_kh_min(g_time_delay_one_pack_min, g_time_delay_one_pack_this);//最小时延
        printf("[iperf-test] receive NO.%"PRIu64" package, time_delay: %"PRIu64" \r\n",
               g_time_delay_send_num_count, (g_time_delay_end - g_time_delay_start));
        if (g_time_delay_send_num_count == g_time_delay_send_num) {
            g_time_delay_one_pack_jitter = g_time_delay_one_pack_max - g_time_delay_one_pack_min;
            printf("[iperf-test], delay end, count-num: g_time_delay_send_num_count: %"PRIu64", avarage: %"PRIu64", "
                   "diff_jitter:%"PRIu64" \r\n", g_time_delay_send_num_count, g_time_delay_count / g_time_delay_send_num_count,
                   g_time_delay_one_pack_jitter);
            g_time_delay_send_num_count = 0;
            g_time_delay_count = 0;
            return;
        }

        ClientWriteParam paramWrite;
        paramWrite.handle = 2;//2
        paramWrite.type = 0;//0
        paramWrite.dataLength = data->dataLength;//460

        uint8_t *data_iperf;
        data_iperf = (uint8_t *)malloc(data->dataLength * sizeof(uint8_t));
        if (data_iperf == NULL) {
            printf("Failed to allocate memory.\n");
            return;
        }
        memset(data_iperf, 0, data->dataLength * sizeof(uint8_t));
        sle_kht_msghdr *msghdr = (sle_kht_msghdr *)data_iperf;
        msghdr->msgid = COUNT_TIME_DELAY_ID;
        msghdr->magic_num = SLE_KHT_MAGIC_NUM;
        msghdr->length = paramWrite.dataLength;

        paramWrite.data = data_iperf;
        sleep(1);

        g_time_delay_start = sle_khtest_clock_gettime_us();
        SleErrorCode ret = SendWriteCmd(clientId, connectId, &paramWrite);

        if(ret != SLE_NO_ERROR) {
            printf("[SleSsapServerTestTool] write failed(%hhu)\r\n", ret);
        }
        free(data_iperf);
    }
//    pthread_mutex_unlock(&tx_mutex);

    pthread_mutex_lock(&tx_mutex);
    switch (msghdr->msgid) {
        case SLE_KHT_COMM_RX_PAYLOAD:
            // 统计流量
            if (0 == client_rx_stat.total_count) {
                client_rx_stat.begin_tv_us = sle_khtest_clock_gettime_us();
                client_rx_stat.total_drops = 0;
                //client_rx_stat.seq_start = msghdr->seqid;
                //client_rx_stat.seq_stop = client_rx_stat.seq_start;
                client_rx_stat.total_bytes = data->dataLength;
                sle_kht_this_pack_arrive = client_rx_stat.begin_tv_us;
                sle_kht_last_pack_arrive = sle_kht_this_pack_arrive;
                sle_kht_max_jitter = 0;
            } else {
                //client_rx_stat.seq_stop = msghdr->seqid;
                client_rx_stat.total_bytes += data->dataLength;
                sle_kht_this_pack_arrive = sle_khtest_clock_gettime_us();
                uint64_t diff_jitter = sle_kht_this_pack_arrive - sle_kht_last_pack_arrive;
                sle_kht_max_jitter = sle_kh_max(sle_kht_max_jitter, diff_jitter);
                sle_kht_last_pack_arrive = sle_kht_this_pack_arrive;
            }

            if (tx_ctx->start_seq == 0) {
                tx_ctx->start_seq = msghdr->seqid; // 记录本轮统计第一个包的序列号
                tx_ctx->last_seq = msghdr->seqid;
            } else {
                do {
                    if (msghdr->seqid < tx_ctx->start_seq) {
                        break; // 旧的报文
                    }

                    //msghdr->seqid > tx_ctx->start_seq
                    if (msghdr->seqid > tx_ctx->last_seq) {
                        if ((msghdr->seqid - tx_ctx->last_seq) > 1) {
                            // 乱序丢包
                            client_rx_stat.total_drops += (msghdr->seqid - tx_ctx->last_seq - 1);
                        }
                        tx_ctx->last_seq = msghdr->seqid;
                        break;
                    }

                    if (msghdr->seqid == tx_ctx->last_seq) {
                        pthread_mutex_unlock(&tx_mutex);
                        return; // 重复的包
                    }

                    if (msghdr->seqid < tx_ctx->last_seq) {
                        // 前面的乱序包, 补偿
                        if (client_rx_stat.total_drops > 0) {
                            client_rx_stat.total_drops --;
                        }
                    }
                } while (0);
            }

            client_rx_stat.total_count += 1;
//            printf("[iperf-test] seqid: %d, data->dataLength: %d\r\n", msghdr->seqid, data->dataLength);
            if ((client_rx_stat.total_count + client_rx_stat.total_drops) >= g_SLE_KHT_IPERF_ONCE_PACK_NUM_S_TO_C) {
                client_rx_stat.end_tv_us = sle_khtest_clock_gettime_us();
                diff_us = (double)(client_rx_stat.end_tv_us - client_rx_stat.begin_tv_us);
                diff_us /= 1000; // =ms
                if ((int)diff_us == 0) diff_us = 1;

                double rate_iperf = (double)((client_rx_stat.total_bytes*8*1024)/diff_us);
                printf("[iperf-test] one_pack_size: %d, rate_iperf: %.3lfbps, cost_time_ms: %.3lfms, "
                       "this_drop %u, this_receive %u, this_pack %u\r\n", data->dataLength, rate_iperf, diff_us,
                       client_rx_stat.total_drops, client_rx_stat.total_count,
                       (client_rx_stat.total_drops + client_rx_stat.total_count));

                client_rx_stat.total_count = 0;
                client_rx_stat.total_bytes = 0;
                client_rx_stat.total_drops = 0;
            }
            break;

        case SLE_KHT_COMM_RX_END:
            g_end_iperf_count++;
            uint16_t connectNum = GetCountConnectedNum();
            if (connectNum == g_end_iperf_count) {
                printf("[iperf-test] recerve end\r\n");
                client_rx_stat.total_count = 0;
                client_rx_stat.total_bytes = 0;
                client_rx_stat.total_drops = 0;
                g_end_iperf_count = 0;
                g_iperfContinueSend = 0;
            }
        default:
            break;
    }
    pthread_mutex_unlock(&tx_mutex);

    pthread_mutex_lock(&tx_mutex);
    if (msghdr->msgid == IPERF_RTT) {
        g_iperf_rtt_count_connectID[connectId] = g_iperf_rtt_count_connectID[connectId] + 1;
        g_time_delay_send_num_count++;

        if (g_time_delay_send_num_count % 20 == 0) {
            uint64_t now_time = sle_khtest_clock_gettime_us();
            uint64_t iperf_spend_time = (now_time - g_iperf_rtt_start_time) / 1000;
            printf("[iperf-test] count, has receive %"PRIu64", iperf_spend_time(ms) %"PRIu64", all_time %"PRIu64" \r\n",
                   g_time_delay_send_num_count, iperf_spend_time, (g_iperf_trr_all_time * 1000));
            if (iperf_spend_time > (g_iperf_trr_all_time * 1000)) {
                printf("[iperf-test] RTT iperf end \r\n");
                g_time_delay_send_num_count = 0;
                pthread_mutex_unlock(&tx_mutex);
                return;
            }
        }

        if (g_time_delay_send_num_count % 500 == 0) {
            for (int i = 0; i < SLE_KHT_MAX_ADDR_RESULTS; ++i) {
                if (g_iperf_rtt_count_connectID[i] > 0) {
                    printf("[iperf-test] connectId: %d, packages: %"PRIu64" \r\n", i, g_iperf_rtt_count_connectID[i]);
                }
            }
        }

        ClientWriteParam paramWrite;
        paramWrite.handle = 2;//2
        paramWrite.type = 0;//0
        paramWrite.dataLength = data->dataLength;//460

        uint8_t *data_iperf;
        data_iperf = (uint8_t *)malloc(data->dataLength * sizeof(uint8_t));
        if (data_iperf == NULL) {
            printf("Failed to allocate memory.\n");
            return;
        }
        memset(data_iperf, 0, data->dataLength * sizeof(uint8_t));
        sle_kht_msghdr *msghdr = (sle_kht_msghdr *)data_iperf;
        msghdr->msgid = IPERF_RTT;
        msghdr->magic_num = SLE_KHT_MAGIC_NUM;
        msghdr->length = paramWrite.dataLength;
        msghdr->conenctId = connectId;

        paramWrite.data = data_iperf;

//        usleep(500000);
        SleErrorCode ret = SendWriteCmd(clientId, connectId, &paramWrite);

        if(ret != SLE_NO_ERROR) {
            printf("[SleSsapServerTestTool] write failed(%hhu)\r\n", ret);
        }
        free(data_iperf);
    }
    pthread_mutex_unlock(&tx_mutex);
}

void TResetStartIperfFromAllServer(void)
{
    uint16_t ack = (uint16_t)SleGetInputNumber("Please input if reset config, 1 is reset");
    if (ack == 1) {
        client_rx_stat.total_count = 0;
        client_rx_stat.total_bytes = 0;
        client_rx_stat.total_drops = 0;
    }
}

void *abnormalRetryConnect(void *addr)
{
    SleDeviceAddress* remote_addr = (SleDeviceAddress*)addr;
    int32_t count = 0;
    sleep(5);
    while (count < RETRY_CONNECT && g_sle_client_conn_state != SLE_CONNECT_CONNECTED_STATE) {
        count++;
        printf("[iperf-test] start NO.%d retry connect addr["MACSTR"] \r\n", count, MAC2STR(remote_addr->addr));
        SleConnect(remote_addr);
        sleep(RETRY_CONNECT_TIME_S);
        if (g_sle_client_conn_state == SLE_CONNECT_CONNECTED_STATE) {
            printf("[iperf-test] retry connect end, state = %s \r\n", SleConnectStateStr(g_sle_client_conn_state));
            return ((void *)0);
        }
    }
    printf("[iperf-test] retry connect end, state = %s \r\n", SleConnectStateStr(g_sle_client_conn_state));
    free(remote_addr);
    return ((void *)0);
}

uint8_t g_clientDelayId = 99;

void OnSleTConnStateChangeDelayCb(uint16_t connectId, const SleDeviceAddress *addr
        , SleConnectState connState, SlePairState pairState, SleDisConnectReason discReason)
{
    g_sle_client_conn_state = connState;
    if (connState == SLE_CONNECT_CONNECTED_STATE){
        tx_context_t *tx_ctx = sle_client_find_valid_ctx_by_connid(connectId);
        if (tx_ctx == NULL) {
            tx_thread_tbl[connectId].connect_id = connectId;
            tx_thread_tbl[connectId].valid = 1;
            tx_thread_tbl[connectId].start_seq = 0;
            tx_thread_tbl[connectId].last_seq = 0;
            memset(tx_thread_tbl[connectId].device_addr, 0, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
            memcpy(tx_thread_tbl[connectId].device_addr, addr->addr, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
//            if (g_iperfContinueSend == 1) {
//
//            }
        } else if (tx_ctx != NULL) {
            tx_thread_tbl[connectId].valid = 1;
        }
    }

    if (connState == SLE_CONNECT_DISCONNECTED_STATE){
        g_time_delay_send_num_count = 0;
        tx_context_t *tx_ctx = sle_client_find_valid_ctx_by_connid(connectId);
        if (tx_ctx != NULL){
            if (tx_thread_tbl[connectId].valid == 1) {
                tx_thread_tbl[connectId].valid = 0;
                //only client can retry connect
                if (discReason != SLE_DISCONNECT_FOR_LOCAL && g_clientDelayId != 99 && g_autoConnect_when_discon == 1) {
                    SleDeviceAddress* paramsAddr = malloc(sizeof(SleDeviceAddress));
                    memset(paramsAddr, 0, sizeof(SleDeviceAddress));
                    memcpy(paramsAddr, addr, sizeof(SleDeviceAddress));
                    printf("[iperf-test] connectId[%d] addr["MACSTR"] disconnect abnormal \r\n",
                           connectId, MAC2STR(paramsAddr->addr));
                    pthread_t retry_connect_thread;
                    int rc;
                    // 创建新线程
                    rc = pthread_create(&retry_connect_thread, NULL, abnormalRetryConnect, (void*)paramsAddr);
                    if (rc) {
                        printf("Error: unable to create thread, %d\n", rc);
                    }
                }

            }
        }
    }
}

void TAddSspaClientDelay(void)
{
    g_autoConnect_when_discon = (uint16_t)SleGetInputNumber("input if auto connect, 1 is yes, 0 is no");
    SleUuid appUuid;
    memcpy(appUuid.uuid, "SleToolSsapcDela", strlen("SleToolSsapcDela"));
    appUuid.len = strlen("SleToolSsapcDela");
    SleErrorCode ret = AddSspaClient(&appUuid, &g_clientDelayId);
    if(ret != SLE_NO_ERROR) {
        printf("[SleSsapClientDelayTestTool] AddSspaClient failed(%hhu)\r\n", ret);
        return;
    }
}

void TSleConnectDelay(void)
{
    SleDeviceAddress remoteAddr = {
            .addrType = SLE_PUBLIC_ADDRESS_TYPE,
    };
    char addr[18];
    SleGetInputString("Please input remote device public addr(00:11:aa:bb:f1:6b)", addr, sizeof(addr));
    if(strlen(addr) != 17) {
        printf("[iperf-Test] SleSetHostAddress failed, invalid addr\r\n");
        return;
    }
    Str2Mac(remoteAddr.addr, addr);
    SleErrorCode ret = SleConnect(&remoteAddr);
    if(ret != SLE_NO_ERROR) {
        printf("[iperf-Test] SleConnect failed, ret:%hhu\r\n", ret);
        return;
    }
}

void TSleUpdateConnParamToAllDelay(void)
{
    SleConnectionParamUpdate param;
//    param.connectId = (uint16_t)SleGetInputNumber("Please input connectId:");
    param.intervalMin = (uint16_t)SleGetInputNumber("Please input intervalMin:");
    param.intervalMax = (uint16_t)SleGetInputNumber("Please input intervalMax:");
    param.maxLatency = (uint16_t)SleGetInputNumber("Please input latency:");
    param.supervisionTimeout = (uint16_t)SleGetInputNumber("Please input supervisionTimeout:");

    for (int i = 0; i < SLE_KHT_MAX_ADDR_RESULTS; ++i) {
        if (tx_thread_tbl[i].valid == 1 && tx_thread_tbl[i].connect_id >= 0){
            sleep(1);
            param.connectId = tx_thread_tbl[i].connect_id;
            SleErrorCode ret = SleUpdateConnParam(&param);
            if(ret != SLE_NO_ERROR) {
                printf("[iperf-test] SleUpdateConnParam failed(%hhu), connectId: %d\r\n",
                       ret, tx_thread_tbl[i].connect_id);
            } else {
                printf("[iperf-test] SleUpdateConnParam ok, connectId: %d\r\n", tx_thread_tbl[i].connect_id);
            }
        }
    }
}

void TSendExchangeInfoReqDelay(void)
{
//    uint16_t connectId = (uint16_t)SleGetInputNumber("please input connectId: ");
    SsapMtuInfo paramExchange;
    paramExchange.mtuSize = (uint32_t)SleGetInputNumber("please input mtuSize: ");

    for (int i = 0; i < SLE_KHT_MAX_ADDR_RESULTS; ++i) {
        if (tx_thread_tbl[i].valid == 1 && tx_thread_tbl[i].connect_id >= 0){
            sleep(1);
            SleErrorCode ret = SendExchangeInfoReq(g_clientDelayId, tx_thread_tbl[i].connect_id, &paramExchange);
            if(ret != SLE_NO_ERROR) {
                printf("[SleSsapClientTestTool] SendExchangeInfoReq failed(%hhu), connectId: %d\r\n",
                       ret, tx_thread_tbl[i].connect_id);
            } else {
                printf("[SleSsapClientTestTool] SendExchangeInfoReq ok, connectId: %d\r\n", tx_thread_tbl[i].connect_id);
            }
        }
    }
}

void TStartIperfFromAllServerDelay(void)
{
    printf("[iperf-test] This func will send to all connected devices, iperf from server to client\n");
    ClientWriteParam paramWrite;
    paramWrite.handle = 2;//2
    paramWrite.type = 0;//0
    int32_t mySendSize = (int32_t)SleGetInputNumber("L2:460, L0:equal one package data_len, eg:460");
    int32_t size_send = (int32_t)SleGetInputNumber("input one package data_len, eg:460");
    g_SEND_STEP_S_TO_C = (uint16_t)SleGetInputNumber("input send how many steps, eg: 1");
    g_ALL_SEND_PACKAGE_S_TO_C = (uint16_t)SleGetInputNumber("input send how many pachages in one step, eg: 1000");
    g_SLE_KHT_IPERF_ONCE_PACK_NUM_S_TO_C = (uint16_t)SleGetInputNumber("inout how many packages are counted can print result");
    g_IPERF_TIME_S_TO_C = (uint32_t)SleGetInputNumber("input iperf time(s), no need input 0");
    g_iperf_long_time_delay_msS_TO_C = (uint32_t)SleGetInputNumber("input iperf delay time(ms), no need input 0");
    uint8_t *data;
    data = (uint8_t *)malloc(mySendSize * sizeof(uint8_t));
    if (data == NULL) {
        printf("Failed to allocate memory.\n");
        return;
    }
    memset(data, 0, mySendSize * sizeof(uint8_t));
    sle_kht_msghdr *msghdr = (sle_kht_msghdr *)data;

    msghdr->msgid = SLE_KHT_COMM_CLIENT_REQ_SERVER;
    msghdr->magic_num = SLE_KHT_MAGIC_NUM;
    msghdr->seqid = 0;
    msghdr->length = size_send;
    msghdr->sendstep = g_SEND_STEP_S_TO_C;
    msghdr->allsendpackage = g_ALL_SEND_PACKAGE_S_TO_C;
    msghdr->oncecount = g_SLE_KHT_IPERF_ONCE_PACK_NUM_S_TO_C;
    msghdr->iperftime = g_IPERF_TIME_S_TO_C;
    msghdr->iperfdelaytime = g_iperf_long_time_delay_msS_TO_C;

    paramWrite.dataLength = mySendSize;
    paramWrite.data = data;

    for (int i = 0; i < SLE_KHT_MAX_ADDR_RESULTS; ++i) {
        if (tx_thread_tbl[i].valid == 1 && tx_thread_tbl[i].connect_id >= 0){
//            usleep(100000);
            SleErrorCode ret = SendWriteCmd(g_clientDelayId, tx_thread_tbl[i].connect_id, &paramWrite);
            if(ret != SLE_NO_ERROR) {
                printf("[iperf-test] SendWriteCmd failed(%hhu), connectId: %d\r\n",
                       ret, tx_thread_tbl[i].connect_id);
            } else {
                printf("[iperf-test] SendWriteCmd ok, connectId: %d\r\n", tx_thread_tbl[i].connect_id);
                g_iperfContinueSend = 1;
            }
        }
    }

}

void *TIperfClientToServerThread(void *arg)
{
    printf("[iperf-test] This func iperf from client to server\n");
    ClientWriteParam paramWrite;
    uint16_t connectId_iperf_to_server = g_connectId_CToS;//0
    paramWrite.handle = 2;//2
    paramWrite.type = 0;//0
    paramWrite.dataLength = g_dataLength_CToS;//460
    int32_t flagIfCmd = g_flagIfCmd;

    uint8_t *data;
    data = (uint8_t *)malloc(paramWrite.dataLength * sizeof(uint8_t));
    if (data == NULL) {
        printf("Failed to allocate memory.\n");
        return NULL;
    }
    memset(data, 0, paramWrite.dataLength * sizeof(uint8_t));
    sle_kht_msghdr *msghdr = (sle_kht_msghdr *)data;

    uint32_t kht_send_Seq = 0;
    uint32_t kht_step = 0;
    uint32_t sle_kht_server_comm_seq = 0;
    uint64_t begin_tv_us = 0, end_tv_us = 0;
    begin_tv_us = sle_khtest_clock_gettime_us();
    uint64_t has_iperf = 0;
    printf("[iperf-test] step: %d, send_one_step_package: %d, data_len: %d\n",
           g_SEND_STEP_S_TO_C, g_ALL_SEND_PACKAGE_S_TO_C, g_dataLength_CToS);
    while (kht_step < g_SEND_STEP_S_TO_C) {
        while (g_sle_client_conn_state == SLE_CONNECT_CONNECTED_STATE && kht_send_Seq <= (g_ALL_SEND_PACKAGE_S_TO_C-1)) {

            msghdr->msgid = SLE_KHT_COMM_CLIENT_IPERF_TO_SERVER;
            msghdr->magic_num = SLE_KHT_MAGIC_NUM;
            msghdr->seqid = ++sle_kht_server_comm_seq;
            msghdr->length = paramWrite.dataLength;
            msghdr->oncecount = g_SLE_KHT_IPERF_ONCE_PACK_NUM_S_TO_C;

            paramWrite.data = data;
            SleErrorCode ret;
            if (flagIfCmd == 1) {
                ret = SendWriteCmd(g_clientDelayId, connectId_iperf_to_server, &paramWrite);
            } else {
                ret = SendWriteReq(g_clientDelayId, connectId_iperf_to_server, &paramWrite);
            }

            if(ret != SLE_NO_ERROR) {
                printf("[SleSsapServerTestTool] write failed(%hhu)\r\n", ret);
                continue;
            }
            kht_send_Seq ++;
        }
        kht_step ++;
        kht_send_Seq = 0;
        end_tv_us = sle_khtest_clock_gettime_us();
        if (g_IPERF_TIME_S_TO_C > 0) {
            has_iperf = (end_tv_us - begin_tv_us) / 1000000;
            if (has_iperf > g_IPERF_TIME_S_TO_C) {
                printf("[iperf-test] the iperf-time is end, time: %"PRIu64" \r\n", has_iperf);
                break;
            }
        }
    }
    end_tv_us = sle_khtest_clock_gettime_us();
    uint16_t client_end_retry = 0;
    while(client_end_retry < SEND_END_RETRY){
        client_end_retry++;
        msghdr->msgid = SLE_KHT_COMM_RX_END;
        msghdr->magic_num = SLE_KHT_MAGIC_NUM;
        msghdr->seqid = ++sle_kht_server_comm_seq;
        msghdr->length = g_dataLength_CToS;

        ClientWriteParam param = {0};
        param.handle = 2;//2
        param.type = 0;//0
        param.dataLength = g_dataLength_CToS;//460
        param.data = data;

        SleErrorCode ret1 = SendWriteCmd(g_clientDelayId, connectId_iperf_to_server, &param);
        if(ret1 != SLE_NO_ERROR) {
            printf("[iperf-test] SendWriteCmd failed(%hhu)\r\n", ret1);
        } else {
            printf("[iperf-test] SendWriteCmd ok, data_len = %d \r\n", g_dataLength_CToS);
        }
    }
    printf("[iperf-test] iperf client to server end, diff_us: %"PRIu64" , one_pack_size: %d, one_pack_send: %"PRIu64", "
           "send_package_one_step: %d, setps: %d, count-print: %d\n", (end_tv_us - begin_tv_us), g_dataLength_CToS,
           (end_tv_us - begin_tv_us)/(g_SEND_STEP_S_TO_C * g_ALL_SEND_PACKAGE_S_TO_C), g_ALL_SEND_PACKAGE_S_TO_C, g_SEND_STEP_S_TO_C,
           g_SLE_KHT_IPERF_ONCE_PACK_NUM_S_TO_C);
    return ((void *)0);
}

void TStartIperfClientToServerDelay(void)
{
    printf("[iperf-test] This func will iperf from client to server\n");
    g_connectId_CToS = (uint16_t)SleGetInputNumber("input the connectId:, eg:0");
    g_flagIfCmd = (int32_t)SleGetInputNumber("input the flag:, 1 is cmd, other is seq");
    g_dataLength_CToS = (uint16_t)SleGetInputNumber("input the send_data_len:, eg:460");
    g_SEND_STEP_S_TO_C = (uint16_t)SleGetInputNumber("input send how many steps, eg: 1");
    g_ALL_SEND_PACKAGE_S_TO_C = (uint16_t)SleGetInputNumber("input send how many pachages in one step, eg: 1000");
    g_SLE_KHT_IPERF_ONCE_PACK_NUM_S_TO_C = (uint16_t)SleGetInputNumber("inout how many packages are counted can print result");
    g_IPERF_TIME_S_TO_C = (uint32_t)SleGetInputNumber("input iperf time(s), no need input 0");
    g_iperf_long_time_delay_msS_TO_C = (uint32_t)SleGetInputNumber("input iperf delay time(ms), no need input 0");

    pthread_t new_thread;
    int rc;
    // 创建新线程
    rc = pthread_create(&new_thread, NULL, TIperfClientToServerThread, NULL);
    if (rc) {
        printf("Error: unable to create thread, %d\n", rc);
    }

}

void TimeCountDelay(void)
{
    g_connectId_CToS = (uint16_t)SleGetInputNumber("input the connectId:, eg:0");
    g_dataLength_CToS = (uint16_t)SleGetInputNumber("input the send_data_len:, eg:460");
    g_time_delay_send_num = 0;
    g_time_delay_send_num = (uint16_t)SleGetInputNumber("input the send package num:, eg:10");

    ClientWriteParam paramWrite;
    uint16_t connectId_iperf_to_server = g_connectId_CToS;//0
    paramWrite.handle = 2;//2
    paramWrite.type = 0;//0
    paramWrite.dataLength = g_dataLength_CToS;//460

    uint8_t *data;
    data = (uint8_t *)malloc(paramWrite.dataLength * sizeof(uint8_t));
    if (data == NULL) {
        printf("Failed to allocate memory.\n");
        return;
    }
    memset(data, 0, paramWrite.dataLength * sizeof(uint8_t));
    sle_kht_msghdr *msghdr = (sle_kht_msghdr *)data;
    msghdr->msgid = COUNT_TIME_DELAY_ID;
    msghdr->magic_num = SLE_KHT_MAGIC_NUM;
    msghdr->length = paramWrite.dataLength;

    paramWrite.data = data;

    g_time_delay_start = sle_khtest_clock_gettime_us();
    SleErrorCode ret = SendWriteCmd(g_clientDelayId, connectId_iperf_to_server, &paramWrite);

    if(ret != SLE_NO_ERROR) {
        printf("[SleSsapServerTestTool] write failed(%hhu)\r\n", ret);
    }
}

void SleSsapClientToolHalperDelay(void)
{
    printf("***SleSsapClientToolHalperDelay Command List****\r\n");
    for (uint16_t i = 0; i < g_ssapClientDelayCmdNum; i++) {
        printf("*     %02d - %-20s     *\r\n", i, g_ssapClientDelayCmdList[i].desc);
    }
    printf("*************************************\r\n");
}

void SleSsapClientToolCmdHandlerDelay(int argc, char **argv)
{
    printf("SleSsapClientToolCmdHandlerDelay in\r\n");
    //试配uart接口的调用方式
    SleResetEnv();
    //从函数名字之后开始处理参数列表
    SleSetBufEnv(argc, argv, 1);
    int index = SleGetInputNumber("Please input cmd index:\r\n");
    if(index < g_ssapClientDelayCmdNum) {
        printf("cmd: %d\r\n", index);
        printf("Execute: %s\r\n", g_ssapClientDelayCmdList[index].desc);
        g_ssapClientDelayCmdList[index].func();
    }
    if(index != 0) g_ssapClientDelayCmdList[0].func();
}

void TGetHasConnectDevice(void)
{

}

static SleDeviceAddress device_addr[SLE_KHT_MAX_ADDR_RESULTS];

void* TConnectThread(void *addrNum)
{
    uint16_t i = *(uint16_t*)addrNum;
    printf("[iperf-test] start connect %d \r\n", i);
    SleErrorCode ret = SleConnect(&device_addr[i]);
    if(ret != SLE_NO_ERROR) {
        printf("[SleConnectTestTool] SleConnect failed, ret:%hhu\r\n", ret);
    }
    free(addrNum);
    return ((void*)0);
}

void TConnectAllDevice(void)
{
    uint8_t device_addr0[SLE_DEVICE_ADDRESS_LEN] = {0x00,0x01,0x09,0x01,0xC0,0x37};
    uint8_t device_addr1[SLE_DEVICE_ADDRESS_LEN] = {0x00,0x01,0x09,0x01,0xBC,0x86};
    uint8_t device_addr2[SLE_DEVICE_ADDRESS_LEN] = {0x00,0x01,0x09,0x01,0xC0,0x41};
    uint8_t device_addr3[SLE_DEVICE_ADDRESS_LEN] = {0x00,0x01,0x09,0x01,0xC0,0x3E};
    uint8_t device_addr4[SLE_DEVICE_ADDRESS_LEN] = {0x00,0x01,0x09,0x01,0xBC,0x82};
    uint8_t device_addr5[SLE_DEVICE_ADDRESS_LEN] = {0x00,0x01,0x09,0x01,0xBC,0x8D};
    uint8_t device_addr6[SLE_DEVICE_ADDRESS_LEN] = {0x00,0x01,0x09,0x01,0xC0,0x33};
    uint8_t device_addr7[SLE_DEVICE_ADDRESS_LEN] = {0x00,0x01,0x09,0x01,0xC0,0x38};
    uint8_t device_addr8[SLE_DEVICE_ADDRESS_LEN] = {0x00,0x01,0x09,0x01,0xBC,0x7C};
    uint8_t device_addr9[SLE_DEVICE_ADDRESS_LEN] = {0x00,0x01,0x09,0x01,0xBC,0x8C};
    for (int i = 0; i < 10; ++i) {
        memset(device_addr[i].addr, 0, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
        device_addr[i].addrType = SLE_PUBLIC_ADDRESS_TYPE;
    }
    memcpy(device_addr[0].addr, device_addr0, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
    memcpy(device_addr[1].addr, device_addr1, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
    memcpy(device_addr[2].addr, device_addr2, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
    memcpy(device_addr[3].addr, device_addr3, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
    memcpy(device_addr[4].addr, device_addr4, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
    memcpy(device_addr[5].addr, device_addr5, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
    memcpy(device_addr[6].addr, device_addr6, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
    memcpy(device_addr[7].addr, device_addr7, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
    memcpy(device_addr[8].addr, device_addr8, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));
    memcpy(device_addr[9].addr, device_addr9, SLE_DEVICE_ADDRESS_LEN * sizeof(uint8_t));

    for (uint16_t i = 0; i < 10; ++i) {
        sleep(1);
        void *serial = malloc(sizeof(uint16_t));
        memset(serial, 0, sizeof(uint16_t));
        memcpy(serial, &i, sizeof(uint16_t));

        pthread_t connect_thread;
        int rc;
        // 创建新线程
        rc = pthread_create(&connect_thread, NULL, TConnectThread, (void*)serial);
        if (rc) {
            printf("Error: unable to create thread, %d\n", rc);
        }
    }

}

void TSleDisConnectDelay(void)
{
    SleDeviceAddress remoteAddr = {
            .addrType = SLE_PUBLIC_ADDRESS_TYPE,
    };
    char addr[18];
    SleGetInputString("Please input remote device public addr(00:11:aa:bb:f1:6b)", addr, sizeof(addr));
    if(strlen(addr) != 17) {
        printf("[iperf-test] SleSetHostAddress failed, invalid addr\r\n");
        return;
    }
    Str2Mac(remoteAddr.addr, addr);
    SleErrorCode ret = SleDisconnect(&remoteAddr);
    if(ret != SLE_NO_ERROR) {
        printf("[iperf-test] SleDisconnect failed, ret:%hhu\r\n", ret);
        return;
    }
}

void TIperf_RTT(void)
{
    g_dataLength_CToS = (uint16_t)SleGetInputNumber("input the send_data_len:, eg:460");
    g_iperf_trr_all_time = 0;
    g_iperf_trr_all_time = (uint64_t)SleGetInputNumber("iperf_rtt spend time: (s)");
    memset(g_iperf_rtt_count_connectID, 0, SLE_KHT_MAX_ADDR_RESULTS * sizeof(uint64_t));

    ClientWriteParam paramWrite;
    paramWrite.handle = 2;//2
    paramWrite.type = 0;//0
    paramWrite.dataLength = g_dataLength_CToS;//460

    uint8_t *data;
    data = (uint8_t *)malloc(paramWrite.dataLength * sizeof(uint8_t));
    if (data == NULL) {
        printf("Failed to allocate memory.\n");
        return;
    }
    memset(data, 0, paramWrite.dataLength * sizeof(uint8_t));
    sle_kht_msghdr *msghdr = (sle_kht_msghdr *)data;
    msghdr->msgid = IPERF_RTT;
    msghdr->magic_num = SLE_KHT_MAGIC_NUM;
    msghdr->length = paramWrite.dataLength;

//    g_time_delay_start = sle_khtest_clock_gettime_us();
    g_iperf_rtt_start_time = sle_khtest_clock_gettime_us();
    printf("[iperf-test] start rtt_iperf, g_iperf_rtt_start_time : %"PRIu64" \r\n", g_iperf_rtt_start_time);
    for (int i = 0; i < SLE_KHT_MAX_ADDR_RESULTS; ++i) {
        if (tx_thread_tbl[i].valid == 1 && tx_thread_tbl[i].connect_id >= 0){
            msghdr->conenctId = tx_thread_tbl[i].connect_id;
            paramWrite.data = data;

            usleep(30000);
            SleErrorCode ret = SendWriteCmd(g_clientDelayId, tx_thread_tbl[i].connect_id, &paramWrite);
            if(ret != SLE_NO_ERROR) {
                printf("[iperf-test] SendWriteCmd failed(%hhu), connectId: %d\r\n",
                       ret, tx_thread_tbl[i].connect_id);
            } else {
                printf("[iperf-test] SendWriteCmd ok, connectId: %d\r\n", tx_thread_tbl[i].connect_id);
            }
        }
    }

}

#ifdef __cplusplus
}
#endif // __cplusplus
